﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using SlimDX;
using SlimDX.DirectInput;

namespace FTFSmartCar
{
    public partial class ServoControlForm : Form
    {

        #region EXPOSED_PROPERTIES

        public bool ManualServoCommandsEnabled
        {
            get { return this.ManualServoCmdCB.Checked;}
            set { this.ManualServoCmdCB.Checked = value;}
        }

        public bool DriveChanged = false;
        public bool SteeringChanged = false;

        public ushort DriveServoNeutral;
        public ushort SteeringServoNeutral;
        public short DriveServoVal;
        public short SteeringServoVal;

        #endregion

        Joystick joystick;
        JoystickState state = new JoystickState();

        public ServoControlForm()
        {
            InitializeComponent();
            SetMaxMin();
            UpdateValuesFromNumericUpDowns();
            LinkControlsToValues();
            UpdateJoystick();
            this.KeyPreview = true;
         
        }

        protected override bool IsInputKey(Keys keyData)
        {
            if (keyData == Keys.Right)
            {
                return true;
            }
            else if (keyData == Keys.Left)
            {
                return true;
            }
            else
            {
                return base.IsInputKey(keyData);
            }
        } 


        private void ServoControlForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            e.Cancel = true;
            this.Hide();
        }

        private void SteeringServoNeutralMinNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        private void SteeringServoValueMinNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        private void DriveServoNeutralMinNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        private void DriveServoValueMinNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        private void SteeringServoNeutralMaxNumericUD_ValueChanged(object sender, EventArgs e)
        {

        }

        private void SteeringServoValueMaxNumericUD_ValueChanged(object sender, EventArgs e)
        {

        }

        private void DriveServoNeutralMaxNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        private void DriveServoValueMaxNumericUD_ValueChanged(object sender, EventArgs e)
        {
            SetMaxMin();
        }

        void SetMaxMin()
        {

            if (DriveServoNeutralMaxNumericUD.Value < DriveServoNeutralMinNumericUD.Value)
            {
                decimal temp;
                temp = DriveServoNeutralMaxNumericUD.Value;
                DriveServoNeutralMaxNumericUD.Value = DriveServoNeutralMinNumericUD.Value;
                DriveServoNeutralMinNumericUD.Value = temp;
            }

            if (DriveServoNeutralMaxNumericUD.Value < DriveServoNeutralMinNumericUD.Value)
            {
                decimal temp;
                temp = DriveServoNeutralMaxNumericUD.Value;
                DriveServoNeutralMaxNumericUD.Value = DriveServoNeutralMinNumericUD.Value;
                DriveServoNeutralMinNumericUD.Value = temp;
            }

            if (SteeringServoNeutralMaxNumericUD.Value < SteeringServoNeutralMinNumericUD.Value)
            {
                decimal temp;
                temp = SteeringServoNeutralMaxNumericUD.Value;
                SteeringServoNeutralMaxNumericUD.Value = SteeringServoNeutralMinNumericUD.Value;
                SteeringServoNeutralMinNumericUD.Value = temp;
            }

            if (SteeringServoValueMaxNumericUD.Value < SteeringServoValueMinNumericUD.Value)
            {
                decimal temp;
                temp = SteeringServoValueMaxNumericUD.Value;
                SteeringServoValueMaxNumericUD.Value = SteeringServoValueMinNumericUD.Value;
                SteeringServoValueMinNumericUD.Value = temp;
            }

            DriveServoNeutralTB.Maximum = (int)DriveServoNeutralMaxNumericUD.Value;
            DriveServoNeutralTB.Minimum= (int)DriveServoNeutralMinNumericUD.Value;

            DriveServoValueTB.Maximum = (int)DriveServoValueMaxNumericUD.Value;
            DriveServoValueTB.Minimum = (int)DriveServoValueMinNumericUD.Value;

            SteeringServoNeutralTB.Maximum = (int)SteeringServoNeutralMaxNumericUD.Value;
            SteeringServoNeutralTB.Minimum = (int)SteeringServoNeutralMinNumericUD.Value;

            SteeringServoValueTB.Maximum = (int)SteeringServoValueMaxNumericUD.Value;
            SteeringServoValueTB.Minimum = (int)SteeringServoValueMinNumericUD.Value;

        }

        private void SteeringServoNeutralTB_Scroll(object sender, EventArgs e)
        {
            UpdateValuesFromTrackBars();
            LinkControlsToValues();
            SteeringChanged = true;
        }

        private void SteeringServoValueTB_Scroll(object sender, EventArgs e)
        {
            UpdateValuesFromTrackBars();
            LinkControlsToValues();
            SteeringChanged = true;
        }

        private void DriveServoNeutralTB_Scroll(object sender, EventArgs e)
        {
            UpdateValuesFromTrackBars();
            LinkControlsToValues();
            DriveChanged = true;
        }

        private void DriveServoValueTB_Scroll(object sender, EventArgs e)
        {
            UpdateValuesFromTrackBars();
            LinkControlsToValues();
            DriveChanged = true;
        }

        private void SteeringServoNeutralValueNumericUD_ValueChanged(object sender, EventArgs e)
        {
            UpdateValuesFromNumericUpDowns();
            LinkControlsToValues();
            SteeringChanged = true;
        }

        private void SteeringServoValueValNumericUD_ValueChanged(object sender, EventArgs e)
        {
            UpdateValuesFromNumericUpDowns();
            LinkControlsToValues();
            SteeringChanged = true;
        }

        private void DriveServoNeutralValNumericUD_ValueChanged(object sender, EventArgs e)
        {
            UpdateValuesFromNumericUpDowns();
            LinkControlsToValues();
            DriveChanged = true;
        }

        private void DriveServoValueValNumericUD_ValueChanged(object sender, EventArgs e)
        {
            UpdateValuesFromNumericUpDowns();
            LinkControlsToValues();
            DriveChanged = true;
        }

        void UpdateValuesFromTrackBars()
        {
            DriveServoNeutral =    (ushort)DriveServoNeutralTB.Value;
            SteeringServoNeutral = (ushort)SteeringServoNeutralTB.Value;
            DriveServoVal =    (short)DriveServoValueTB.Value;
            SteeringServoVal = (short)SteeringServoValueTB.Value;
        }

        void UpdateValuesFromNumericUpDowns()
        {
            DriveServoNeutral = (ushort)DriveServoNeutralValNumericUD.Value;
            SteeringServoNeutral = (ushort)SteeringServoNeutralValueNumericUD.Value;
            DriveServoVal = (short)DriveServoValueValNumericUD.Value;
            SteeringServoVal = (short)SteeringServoValueValNumericUD.Value;
          
        }

        void LinkControlsToValues()
        {
            if((ushort)DriveServoNeutralTB.Value != DriveServoNeutral) 
                 DriveServoNeutralTB.Value = (int)DriveServoNeutral;
            if ((short)DriveServoValueTB.Value != DriveServoVal)
                DriveServoValueTB.Value = (int)DriveServoVal;
            if ((ushort)SteeringServoNeutralTB.Value != SteeringServoNeutral)
                SteeringServoNeutralTB.Value = (int)SteeringServoNeutral;
            if ((short)SteeringServoValueTB.Value != SteeringServoVal)
                SteeringServoValueTB.Value = (int)SteeringServoVal;

            if ((ushort)DriveServoNeutralValNumericUD.Value != DriveServoNeutral)
                DriveServoNeutralValNumericUD.Value = (int)DriveServoNeutral;
            if ((short)DriveServoValueValNumericUD.Value != DriveServoVal)
                DriveServoValueValNumericUD.Value = (int)DriveServoVal;
            if ((ushort)SteeringServoNeutralValueNumericUD.Value != SteeringServoNeutral)
                SteeringServoNeutralValueNumericUD.Value = (Decimal)SteeringServoNeutral;
            if ((short)SteeringServoValueValNumericUD.Value != SteeringServoVal)
                SteeringServoValueValNumericUD.Value = (Decimal)SteeringServoVal;
        }

        void CreateDevice()
        {
            // make sure that DirectInput has been initialized
            DirectInput dinput = new DirectInput();

            // search for devices
            foreach (DeviceInstance device in dinput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly))
            {
                // create the device
                try
                {
                    joystick = new Joystick(dinput, device.InstanceGuid);
                    joystick.SetCooperativeLevel(this, CooperativeLevel.Exclusive | CooperativeLevel.Foreground);
                    break;
                }
                catch (DirectInputException)
                {
                }
            }

            if (joystick == null)
            {
                MessageBox.Show("There are no joysticks attached to the system.");
                return;
            }

            foreach (DeviceObjectInstance deviceObject in joystick.GetObjects())
            {
                if ((deviceObject.ObjectType & ObjectDeviceType.Axis) != 0)
                    joystick.GetObjectPropertiesById((int)deviceObject.ObjectType).SetRange(-600, 600);
            }

            SteeringTB.Maximum = 600;
            SteeringTB.Minimum = -600;
            ThrottleTB.Maximum = 600;
            ThrottleTB.Minimum = -600;

            // acquire the device
            joystick.Acquire();

            timer.Interval = 1000 / 100;
            timer.Start();
        }

        void ReadImmediateData()
        {
            if (joystick.Acquire().IsFailure)
                return;

            if (joystick.Poll().IsFailure)
                return;

            state = joystick.GetCurrentState();
            if (Result.Last.IsFailure)
                return;

            UpdateJoystick();
        }

        void ReleaseDevice()
        {
            timer.Stop();

            if (joystick != null)
            {
                joystick.Unacquire();
                joystick.Dispose();
            }
            joystick = null;
        }
   
    
        void UpdateJoystick()
        {
            if (joystick == null)
                CreateDeviceBtn.Text = "Create Device";
            else
                CreateDeviceBtn.Text = "Release Device";

            SteeringTB.Value = state.X * FlipSteeringAxisMultiplier();

          
            if (JoystickEnableCB.Checked)
            {
                SteeringServoValueTB.Value = state.X * FlipSteeringAxisMultiplier();

                bool[] ControllerButtons = state.GetButtons();
                if (ControllerButtons[0] == true)
                {
                    if (ControllerButtons[6] == true)
                    {
                        DriveServoValueTB.Value = (int)(500 * ((double)JoystickGain.Value / 100.0));

                    }
                    else
                    {
                        DriveServoValueTB.Value = (int)(500 * ((double)JoystickGain.Value / 200.0));
                    }

                     
                }
                else if (ControllerButtons[1] == true)
                {
                    if (ControllerButtons[6] == true)
                    {
                        DriveServoValueTB.Value = (int)(-500 * ((double)JoystickGain.Value / 100.0));

                    }
                    else
                    {
                        DriveServoValueTB.Value = (int)(-500 * ((double)JoystickGain.Value / 200.0));
                    }

                }
                else
                {
                    DriveServoValueTB.Value = (int)(state.RotationZ * FlipThrottleAxisMultiplier() * ((double)JoystickGain.Value / 100.0));

                }
               

                UpdateValuesFromTrackBars();
                LinkControlsToValues();
                SteeringChanged = true;
                DriveChanged = true;
            }
        }

        int FlipSteeringAxisMultiplier()
        {
            if (FlipSteeringAxisCB.Checked)
                return -1;
            else
                return 1;
        }

        int FlipThrottleAxisMultiplier()
        {
            if (FlipThrottleAxisCB.Checked)
                return -1;
            else
                return 1;
        }

        private void exitButton_Click(object sender, EventArgs e)
        {
            ReleaseDevice();
            Close();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            if (joystick == null)
                CreateDevice();
            else
                ReleaseDevice();
            UpdateJoystick();
        }

        private void timer_Tick_1(object sender, EventArgs e)
        {
            ReadImmediateData();
        }

        private void ServoControlForm_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyCode == Keys.Left)
            {
                this.SteeringServoValueTB.Value = -500;
            }

            if (e.KeyCode == Keys.Right)
            {
                this.SteeringServoValueTB.Value = 500;
            }

            if (e.KeyCode == Keys.A)
            {
                this.DriveServoValueTB.Value = (int)(500 * ((double)JoystickGain.Value / (double)100));
            }

            if (e.KeyCode == Keys.S)
            {
                this.DriveServoValueTB.Value = (int)(-500 * ((double)JoystickGain.Value / (double)100)); ;
            }

            e.Handled = true;

            UpdateValuesFromTrackBars();
            LinkControlsToValues();
        }

        private void ServoControlForm_KeyUp(object sender, KeyEventArgs e)
        {
            if (e.KeyCode == Keys.Left)
            {
                this.SteeringServoValueTB.Value = 0;
            }
            if (e.KeyCode == Keys.Right)
            {
                this.SteeringServoValueTB.Value = 0;
            }

            if (e.KeyCode == Keys.A)
            {
                this.DriveServoValueTB.Value = 0;
            }

            if (e.KeyCode == Keys.S)
            {
                this.DriveServoValueTB.Value = 0;
            }
            UpdateValuesFromTrackBars();
            LinkControlsToValues();

            e.Handled = true;
        }

        private void ServoControlForm_KeyPress(object sender, KeyPressEventArgs e)
        {
            e.Handled = true;
        }


    }
}
